#pragma systemFile
#include "JoystickDriver.c"
bool red=false;

const float ratio=3/2;
const int encoderresolution=1440;
const int wheelsize=4;

int in_enc(int inches)
{
  int ticks;
  ticks = 99+inches/(PI*wheelsize)*encoderresolution*ratio;
  return ticks;
}

void pause()
{
  wait10Msec(50);
}

const int lgrab=180;
const int lopen=75;
const int rgrab=115;
const int ropen=215;

int waittime=0;

void chooser()
{
  wait10Msec(10);
  bDisplayDiagnostics=false;
  eraseDisplay();
  while (nNxtButtonPressed!=kEnterButton)
  {
    nxtDisplayBigTextLine(3,"Wait: %i",waittime);
    if (red==false)
    {
      nxtDisplayBigTextLine(1,"Blue  ");
    }
    if (red==true)
    {
      nxtDisplayBigTextLine(1,"Red ");
    }
    if (nNxtButtonPressed==kLeftButton)
    {
      red=!red;
      wait10Msec(20);
    }
    if (nNxtButtonPressed==kRightButton)
    {
      waittime++;
      wait10Msec(20);
    }
  }
  eraseDisplay();
  bDisplayDiagnostics=true;
}


#include "drivers\hitechnic-angle.h"
//IR Routines

/*

#ifndef max
#define max( a, b ) ( ((a) > (b)) ? (a) : (b) )
#endif

#include "drivers\HTIRS2-driver.h"
float currDir;
float IRGetACDir(tSensors link)
{
  static float prevDir = 0.0;

  int acS[5];
  int idx;

  idx = HTIRS2readACDir(link);
  currDir = (float)idx;
  if (idx == 0)
  {
    currDir = prevDir;
  }
  else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4]))
  {
    idx = (idx - 1)/2;
    if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
    {
      currDir += (float)(acS[idx + 1] - acS[idx])/
      max(acS[idx], acS[idx + 1]);
    }
    nxtDisplayTextLine(0, "Idx=%d,Dir=%5.1f", idx, currDir);
    nxtDisplayTextLine(2, "S1=%d,S2=%d", acS[0], acS[1]);
    nxtDisplayTextLine(3, "S3=%d,S4=%d", acS[2], acS[3]);
    nxtDisplayTextLine(4, "S5=%d", acS[4]);
  }
  prevDir = currDir;

  return currDir;
}

float dir=0;

void irmove()
{
  ClearTimer(T1);
  while(time100[T1]<70)
  {//dir=IRGetACDir(IR1)-(10-IRGetACDir(IR2));
    nxtDisplayBigTextLine(5,"%f",dir);
    if (dir<0)
    {
      motor[FR]=-50;
      motor[RR]=-50;
      motor[FL]=50;
      motor[RL]=50;
    }
    if (dir>0)
    {
      motor[FR]=50;
      motor[RR]=50;
      motor[FL]=-50;
      motor[RL]=-50;
    }
    if (dir==0)
    {
      motor[FR]=50;
      motor[RR]=50;
      motor[FL]=50;
      motor[RL]=50;
    }
  }
}
*/

//Motion Routines

	void straight(int power, int encoder)
	{
	  int currentPower = 0;

	  nMotorEncoder[FR]=0;
	  nMotorEncoder[RR]=0;
	  nMotorEncoder[FL]=0;
	  nMotorEncoder[RL]=0;

	  while (abs(nMotorEncoder[FR])<encoder||abs(nMotorEncoder[FL])<encoder||abs(nMotorEncoder[RR])<encoder||abs(nMotorEncoder[RL])<encoder)
	  {
	    if(abs(currentPower) < abs(power))
	    {
	      if(power>0)
	      {
	      currentPower++;
	      }
	      if(power<0)
	      {
	      currentPower--;
	      }
	      wait1Msec(20);
	    }
	    motor[FR]=currentPower;
	    motor[RR]=currentPower;
	    motor[FL]=currentPower;
	    motor[RL]=currentPower;
	  }
	  motor[FR]=0;
	  motor[RR]=0;
	  motor[FL]=0;
	  motor[RL]=0;
	}

	void tleft(int power, int encoder)
	{
	  nMotorEncoder[FR]=0;
	  nMotorEncoder[RR]=0;
	  nMotorEncoder[FL]=0;
	  nMotorEncoder[RL]=0;
	  while (abs(nMotorEncoder[FR])<encoder||abs(nMotorEncoder[FL])<encoder||abs(nMotorEncoder[RR])<encoder||abs(nMotorEncoder[RL])<encoder)
	  {
	    motor[FR]=power;
	    motor[RR]=power;
	    motor[FL]=-power;
	    motor[RL]=-power;
	  }
	  motor[FR]=0;
	  motor[RR]=0;
	  motor[FL]=0;
	  motor[RL]=0;
	}
	void tright(int power, int encoder)
	{
	  nMotorEncoder[FR]=0;
	  nMotorEncoder[RR]=0;
	  nMotorEncoder[FL]=0;
	  nMotorEncoder[RL]=0;
	  while (abs(nMotorEncoder[FR])<encoder||abs(nMotorEncoder[FL])<encoder||abs(nMotorEncoder[RR])<encoder||abs(nMotorEncoder[RL])<encoder)
	  {
	    motor[FR]=-power;
	    motor[RR]=-power;
	    motor[FL]=power;
	    motor[RL]=power;
	  }
	  motor[FR]=0;
	  motor[RR]=0;
	  motor[FL]=0;
	  motor[RL]=0;
	}
	void turn(int powert, int encodert)
{
  if (!red)
  {
    tright(powert,encodert);
  }
  else
  {
    tleft(powert,encodert);
  }
}
int position;
int target;
bool positioned;
task PositionArm()
{
  while(true)
  {
    wait1Msec(5);
    position=HTANGreadAccumulatedAngle(angle);
    if((position-target)>8)
    {
      motor[Arm]=-100;
      positioned=false;
    }
    else if((position-target)<-8)
    {
      motor[Arm]=100;
      positioned=false;
    }
    else if((position-target)>2)
    {
      motor[Arm]=-20;
      positioned=false;
    }
    else if((position-target)<-2)
    {
      motor[Arm]=20;
      positioned=false;
    }
    else
    {
      motor[Arm]=0;
      positioned=true;
    }
  }
}

void MoveArm(int goal)
{
  hogCPU();
  target=goal;
  releaseCPU();
}
typedef enum
{
  upper=-118,
  lower=-135,
  inital=75,
  place=70
} positions;
